#ifndef IMU_HPP
#define IMU_HPP
#include <glog/logging.h>
#include <ros/ros.h>
#include <sensor_msgs/Imu.h>
#include <sensor_msgs/Range.h>
#include <std_srvs/SetBool.h>

#include "serial.hpp"
#pragma pack(1)
struct ImuFrame {
    uint8_t header;           // 1
    uint16_t address;         // 2
    uint16_t command;         // 2
    uint16_t length;          // 2
    uint32_t time;            // 4
    float raw_accel[3];       // 12
    float corrected_accel[3]; // 12
    float raw_gyro[3];        // 12
    float corrected_gyro[3];  // 12
    float scaled_gyro[3];     // 12
    float quaternion[4];      // 16
    float euler_angle[3];     // 12
    uint16_t crc;             // 2
    uint16_t end;             // 2
};
#pragma pack()
class Imu : public Serial {
  public:
    Imu(std::shared_ptr<ros::NodeHandle> node_handle_ptr,
        const std::string &serial_port);
    ~Imu() = default;
    Imu(const Imu &) = default;
    Imu &operator=(const Imu &) = default;

    void ImuRxThread();

    void ParseData(std::vector<uint8_t> &data);

    void Publish(const sensor_msgs::Imu &imu);

    bool SetRunning(const bool &running);

    bool SwitchCallBack(std_srvs::SetBool::Request &req,
                        std_srvs::SetBool::Response &res);

  private:
    std::shared_ptr<ros::NodeHandle> node_handle_ptr_;
    ros::Publisher publisher_;
    ros::ServiceServer switch_;
    std::string name_;
    bool is_running_{false}; // 程序是否运行
    bool is_start_{false};   // 硬件是否运行
    // bool is_right_bandrate_{false};
    std::vector<ros::Time> times_;
};
#endif